#pragma once
#ifndef ASTAR_H
#define ASTAR_H

#include "basic_typedef.h"
#include "CollisionDetector.h"
#include <CGAL/Cartesian.h>
#include <iostream>
#include "Prm.h"
#include "CollisionDetector.h"
#include "Oracle.h"

class AStarNode
{
public:
	Point_d point;
	double g_value;
	double f_value;
	bool valid;
	int came_from;
};

class AStar
{
public:
	AStar(Point_d start_point, Point_d target_point, Oracle* oracle, CollisionDetector* col);
	~AStar() {};

	vector<ConfSet> get_path();
	ConfSet get_conf_set(Point_d point);

private:
	double estimation_function(Point_d a, Point_d b);
	int get_current_best_node_index();
	bool is_in_close_list(Point_d point);
	vector<ConfSet> get_full_path(AStarNode node);

private:
	Point_d _start_point, _target_point;
	ConfSet _target_set;
	vector<Point_d> _close_set;
	vector<AStarNode> _open_set;
	Oracle* _oracle;
	CollisionDetector* _col;
};

#endif